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ABSTRACT 

A  pure  pursuit  guidance  law  and  a  heading  autopilot  are  coupled  in  order  to  provide 
path  control  of  submersibles  or  surface  ships  in  the  horizontal  plane.  Proper  design  of  the 
combined  scheme  allows  for  accurate  path  keeping  during  straight  line  motion.  The 
simulation  results  are  extended  to  cover  cases  of  step  changes  in  the  desired  path.  The 
scheme  provides  a  viable  alternative  to  cross  track  error  autopilots. 
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I.   INTRODUCTION 

In  any  operational  scenario  of  an  underwater  vehicle  there  exits  a  triple-nested 
sequence  of  mission  accomplishment  operations:  Path  planning,  navigation,  guidance,  and 
autopilot  design.  The  path  planner  takes  information  from  charted  obstacles  and  friendly 
or  hostile  environments  and  generates  a  smooth  plan  for  the  vehicle  to  follow.  A  certain 
level  of  feedback  exists  in  this  operation  through  the  use  of  sonar  beams  in  order  to 
replan  a  path  when  uncharted  objects  are  encountered  or  when  the  mission  requirements 
have  changed.  Based  on  the  desired  vehicle  positions  and  orientations  at  certain  points, 
several  classes  of  smooth  paths  containing  sets  of  straight  line  segments,  and  circular  arcs 
or  cubic  splines  can  be  obtained  [1].  Once  a  smooth  path  is  generated,  the  navigator 
provides  through  a  selected  guidance  law  the  appropriate  vehicle  heading  commands 
which  are  in  turn  delivered  by  the  autopilot.  Line  of  sight  guidance  using  a  discrete  series 
of  way  points  was  studied  by  Lienard  in  [2]  using  sliding  mode  heaving,  propulsion,  and 
depth  keeping  autopilots.  The  scheme  demonstrated  excellent  stability  and  robustness 
characteristics,  although  the  actual  vehicle  response  was  found  to  lag  significantly  the 
commanded  straight  line  paths.  The  guidance  and  autopilot  functions  can  be  combined 
when  the  lateral  deviation  off  the  desired  path  is  directly  incorporated  into  the  control  law 
design.  This  leads  to  the  development  of  a  cross  track  error  autopilot.  Such  schemes  have 
been  studied  by  Chism  [3]  and  Hawkinson  [4]  for  the  single  input  and  multiple  input 


case,  respectively.  Cross  track  error  autopilots  provide  more  accurate  path  keeping 
response  but  they  must  be  designed  more  carefully  since  they  tend  to  be  more  dependent 
than  heading  controllers  on  an  accurate  description  of  the  vehicle  hydrodynamic 
characteristics.  The  main  reason  for  this  is  the  increase  in  the  system  dimensionality  by 
one.  Underwater  vehicles  operate  in  changing  environments  over  a  wide  range  of 
operating  speeds  and,  therefore,  a  certain  degree  of  uncertainty  exists  in  the  vehicle 
dynamic  modeling.  Cross  track  error  autopilots  also  require  accurate  positional 
information  updates  at  the  same  rate  as  heading  and  heading  rate. 

For  these  reasons,  in  this  work  we  go  back  to  the  case  of  a  heading  autopilot 
coupled  with  an  orientation  guidance  law.  The  two  main  tasks  on  which  we  will 
concentrate  are  as  follows:  First,  we  must  develop  a  way  of  establishing  stability  of  the 
combined  autopilot/guidance  scheme  for  straight  line  commanded  motions.  Second,  the 
actual  vehicle  response  characteristics  must  be  made  to  resemble  the  desired  cross  track 
error  response  with  smooth  transitions  between  consecutive  straight  line  segment  and  with 
minimal  path  overshoot.  A  linear  full  state  feedback  control  law  is  used  to  adjust  the 
heading  of  the  vehicle  to  any  desired  value,  and  a  pure  pursuit  guidance  law  is  used  to 
provide  the  commanded  heading  for  straight  line  motion.  In  this  scheme  the  vehicle 
commanded  heading  equals  the  line  of  sight  angle  between  the  vehicle  center  and  a  target 
point  moving  on  the  desired  path  at  a  constant  lookahead  distance  from  the  vehicle.  This 
parallels  the  case  studied  in  [5]  except  that  in  our  case  the  existence  of  lateral  and 
rotational  dynamics  add  more  complications  to  the  problem.  All  computations  are 
perfomied  for  the  Swimmer  Delivery  Vehicle  [6]  for  which  a  set  of  hydrodynamic 


characteristics  and  geometric  properties  is  available.  Problem  formulation  and  equations 
of  motion  are  presented  in  Section  II.  The  analysis  procedure  is  outlined  in  Section  HI, 
and  simulation  results  are  presented  in  the  Section  IV.  Finally,  conclusions  and 
recommendations  for  further  research  are  given  in  Section  V. 


II.   DEVELOPMENT  OF  THE  MATHEMATICAL  MODEL 

A.      EQUATIONS  OF  MOTION 

In  a  moving  coordinate  frame  fixed  at  the  vehicle  center,  Newton's  equations  of 
motion  for  a  rigid  body  in  the  horizontal  plane  are 

m(y  +ur  +xGr-yGr2)    =    Y,  (2.1) 

Ij  +mxG{v  +ur)  -myGvr     =    N  ,  (2.2) 

where 

v  =  sway(lateral)  velocity, 

r  =  yaw(angular)  velocity, 

u  =  forward(surge)  speed, 

Y  =  sway  force, 

N  =  yaw  moment, 

m  =  vehicle  mass, 

Iz  =  vehicle  mass  moment  of  inertia, 

(Xq^o)  =  coordinates  of  center  of  gravity. 

Expanding  the  force  Y  and  moment  N  in  added  mass,  damping,  and  drag  terms, 
equations  (2.1)  and  (2.2)  are  written  as 


{v+ur+xj-yr2)    =    £/  AY r  +  £./  \Y .v  +  r  wr) 


(2-3) 


+ 

2 


v  +;cr 


/^  +  mxG( v  +  Mr)  -  myGvr    =    _/  5iVr  +  — /  3(N#v  +Nur) 


£/3N«v4Jc^)f^^+£/V2s. 


(2.4) 


v  +jcr 


where 

p  =  water  density, 

1  =  vehicle  length, 

5  =  rudder  angle, 

h(x)  =  vehicle  height  distribution, 

CDy  =  drag  coefficient. 

The  inertial  position  of  the  vehicle  (x,y)  and  its  heading  angle  vy  (  see  Figure  1  ) 
are  given  by 


y    =    r,  (2-5) 


x    -    Mcosy-vsiny  ,  (2.6) 


y     =    Msin\j/+vcos\|/  .  (2.7] 


t-*- 


TARGET  (STEERING  POINT) 


Figure  1    Top  view  of  the  vehicle 


B.       STATE  SPACE  EQUATIONS 

Choosing  (  y,  v,  r  )  as  the  state  vector,  the  linearized  state  space  equations  (2.3), 
(2.4),  and  (2.5)  are  written  as 


\|/     =    r  , 


The  coefficients  in  equations  (8),  (9),  and  (10)  are  given  by 


(/r-0.5p/  5W.)(0.5p/  2yv.)  -(mXc  -0.5p/  4y  .)(0.5p/  W) 
a. 


ii 


an 


a2X 


all 


b,     = 


(/z-0.5p/  5N.)(m  -0.5p/  3y .)  -(mXG-0.5p/  *y  .)(mXG-0.5p/  4/V.) 

(/r -0.5p/  5N.)( -m  +0.5p/  3yr)  -(mXG -0.5p/  4y .)( -mXG  +0.5p/  4N  ) 
(/r  -0.5p/  5N.)(m  -0.5p/  3y .)  -(mXG  -0.5p/  4y.)(mXG -0.5p/  4N.) 

(mXG  -0.5p/  4yr)(0.5p/  Wv)  -(mXG-0.5pl  AN,)(  -m  +0.5p/  2yv) 
(/,  -0.5p/  5/V.)(m  -0.5p/  3y  v.)  -(mXG-0.5p/  4yr)(mXG  -0.5p/  4/V ,) 

(mXG  -0.5p/  4y,)(  -mXG  +0.5p/  4yV)  -(mXG  -0.5p/  47V .)( -m  +0.5p/  3vr) 
(7r  -0.5p/  5N.)(/n  -0.5p/  3j .)  -{mXG  -0.5p/  4y,;)(mXc  -0.5p/  4W.) 

(/.  -0.5p/  57V.)(0.5p/  2y6)  -(mXG  -0.5p/  4y  ,)(0.5p/  W5) 
(/.  -0.5p/  5N.){m  -0.5p/  3v ..)  -(mXG-0.5p/  V)(mXG  -0.5p/  4N,) 


(2.8) 


v     =    auuv+al2ur+blu2S  ,  (2.9) 


r     =    a2luv+a22ur+b2u2b  .  (2.10) 


_  (mXG  -0.5p/  V)(0.5p/  >NB)  -(mXG  -0.5p/  4^)(0.5p/  2y6) 

(F  -0.5p/  W.)(m  -0.5p/  V)  -(mXG-0.5p/  iy,)(mXG  -0.5p/  W .) 


where 

yg,  yr,  yv,  y^,  yrf    =    lateral  hydrodynamic  coefficients 

Nh,  Nr,  Nv,  N.,  N.    =    yaw  hydrodynamic  coefficients 

Equations  (2.8),  (2.9),  and  (2.10)  describe  the  dynamics  of  the  system  with  respect 
to  small  deviations  around  a  nominal  direction  \\f  =  0  . 

C.       PATH  KEEPING  DEVELOPMENT 

1.       Heading  control 

A  linear  full  state  feedback  control  law  is  of  the  form 

6    =    k^+k2v+k3r  (2.11) 

where  k,,  kj  and  k3  are  the  three  gains. 

From  equation  (2.8),(2.9),(2.10)  and  (2.11),  the  closed  loop  system  is 


\\f     =    r 


(2.12) 


v     =    blu2kl\\f+(auii+blu2k2)v+(al2u+blu2k})r  (2.13) 


r    =    b2u  2kx\\f+(a2Xu+b2u  2k2)v  +(a22u  +b2u  2k3)r 


(2.14) 


The  characteristic  equation  of  (2.12),  (2.13)  and  (2.14)  is 


0-X 


0 


bxu2kx     axxu+bxu2k2-X       aX2u+bxu2k3 
b2u  2k2       a2lu+b2u  2k2       a22u  +b2u  2k3  -X 


0 


X[{axxu  +bxu  2k2-X)(a22u  +b2u  2k3  -X)-(anu  +bxu  2k3)(a2xu  +b2u  2k2)] 
-bxu  2kl(a2lu+b2u  2k2)+b2u  2kx(axxu+bxu  2k2-X) 


=    0 


X[aua22u2+alib2u3ki-hiuu+bla22u*k2+blb2u4k2k3-)d?lu2k2-)ui22u 
-Vb2u  2k3  +X2  -ax2a2Xu 2  -aX2b2u  3k2  -a2Xbxu  *k3  -bxb2u  4k2k3] 
-bxa2xu  3kx  -bxb2u  *kxk2+axxb2u  3kx  +bxb2u  ikxk2-'Kb2u  2kx 


=    0 


X3-(axxu+bxu  2k2+a22u+b2u  2k3)X2+{axxa22u2+axxb2u  *k3+bxa22u  3k2 
-ax2a2Xu2-anb2u  *k2-a2Xbxu  3k3-b2u  2kx)\+b2anu  3kx  -bxa2Xu  3kx 


=    0        (2.15) 


2.       Desired  characteristic  equation 

The  3rd  order  ITAE  polynomial  is  defined  by 


X3+1.75(00^2+2.15cOoA.+co;    =    0 


(2.16) 


©0    =    10— 


where 

tc  =  settling  time  (dimensionless) 
From  equations  (2.15)  and  (12.6)  we  get 


-auu-blu2k2-a22u-b2u2k3     =    1.75oo0  (2.17) 


aua22u 2  +aub2u  %  +bxa22u  \  -al2a2iu 2 

-aX2b2u\-a2Xbxu%-b2u2kx    =    2.15©?  (2.18) 


b2auu'krbxa2Xu\     =    too  (2-19) 


The  system  of  equations  (2.17),  (2.18)  and  (2.19)  can  be  solved  for  the  three  gains 


k 


co0 

b2axy-bxa2y 


k2    -     ^-l^^„^Xfl„6a^A)^Ul5^-llV^AiM»^^) 

bxu*(flnb2-a2fid-b2u\anbx-al2bj 

b2u2[2A5(Dl-u2(axxa22-ax2a2x)+b2u2kx]-u\-lJ5(^l-axxu-a22u)(a22bx-aX2b2) 


t 


byia^-a^b^-b^ia^b^a^) 
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3.       Pure  pursuit  navigation 

For  a  pursuit  navigation 

\|/c     =    a  (2.20) 

where 

\|/c  =  commanded  heading 
Referring  to  Figure  1 ,  the  line  of  sight  angle  a  is  defined  by 


a     =     -tan  ' 


xJ  (2.21) 


^d 


where  xd  is  the  vehicle  lookahead  distance,  and  the  control  law  (2.11)  becomes 

5    =    k^y-yj+ty+kf  (2.22) 

Using  equations  (2.20),  (2.21)  and  (2.22)  we  get 


5    =    k.(}ir+tan~l—)+k,v+k-r 

1  T  xd      2      3  (2.23) 

The  linearized  equation  (2.23)  becomes 


5     =    fc)(\y+-^-)+fc2v+fc3r 


%;     2      3  (2.24) 


The  linearized  equation  for  the  lateral  deviation  y  is  obtained  from  (2.7)  as 


y     =    «y+v  (2.25) 
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Now  the  complete  state  vector  is  y,  v,  r  and  y  ,  and  the  state  space  equations  are  written 
as 


y     =    r 


(2.26) 


v     = 


b  xu  2kl\\f  +(a  uu  +b  lu2k2)v  +{a  nu  +b  iu2k})r  +b  iu2kl — y 


(2.27) 


r     =    b2u2kx\y+{a2xu+b2u2k2)v+{a22u+b2u2kjr+b2u2kx — y 


(2.28) 


y     =    «\|/+v 


(2.29) 


and  the  characteristic  equation  is 


0-A. 


0 


0 


byu2kx     auu+blu2k2-X       al2u+bxu2k3       blu2kl — 


b2u2k2       a2xu+b2u2k2       a22u+b2u2k3-X     b2u2kx — 


1 


0 


0-X 


(2.30) 
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in.   COMPUTER  SIMULATION 

A.       PROGRAMMING 

1.  Program  in  MATRIX.X 

The  MATRIX.X  software  is  in  VAX/VMS  in  the  Mechanical  Engineering 
Department.  The  program  in  MATRIX.X  is  written  to  find  gains  and  poles  of  the  system 
from  the  given  inputs,  forward  speed  (u),  settling  time  (tc)  and  vehicle  lookahead  distance 
(xd).  This  is  used  to  compute  the  eigenvalues  of  the  complete  system  (  Equation  (2.30). 
The  four  eigenvalues  of  the  system  are  computed  for  the  given  values  of  u,  tc  and  xd. 
For  stable  vehicle  response,  all  four  must  be  negative  (or  have  negative  real  parts).  If  at 
least  one  is  positive,  the  vehicle  response  will  be  unstable  and  convergence  to  the  straight 
line  path  is  not  to  be  expected.  In  such  a  case,  the  parameters  (in  particular  the  lookahead 
distance  must  be  changed)  so  that  the  vehicle  is  stable.  A  listing  of  this  program  is 
presented  in  Appendix  A. 

2.  Program  in  FORTRAN 

The  first  program  in  FORTRAN,  is  written  to  find  distance  along  body  fixed 
axis  (  x-y  )  from  inputs,  heading  angle  (y),  perpendicular  distance  from  vehicle  to  route 
(y),  yaw  rate  (r),  forward  speed  (u),  settling  time  (tc)  and  vehicle  lookahead  distance  (xd). 
A  modified  version  of  this  simulation  program  is  presented  in  Appendix  B.  The  modified 
version  is  used  to  control  the  vehicle  position  in  a  general  (  X-Y  )  inertia!  system.  The 
desired  vehicle  path  is  discretized  into  a  series  of  straight  line  segments  and  the  same 
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lookahead  distance  xd  is  used  to  regulate  the  vehicle  deviation  of  each  segment.  Details 
of  this  modification  are  presented  in  the  next  paragraph. 

3.       Graphics 

The  GRAFSTAT  graphic  package  is  used  to  produce  2  dimensional  graphs 
by  using  data  from  the  simulation  programs. 


B.       DETAILS 

1.  Poles  and  gains 

Calculate  the  system  poles  and  gains  for  a  given  forward  speed  u  and  various 
combinations  of  tc  and  xd.  Select  tc  and  xd  such  that  appropriate  (sufficiently  negative) 
poles  and  gains  for  the  system  are  produced.  This  is  verified  by  repeated  simulations  from 
step  2  that  follows 

2.  Distance  in  x-y  axis 

Using  the  values  of  tc  and  xd  from  the  previous  step,  the  system  response  can 
be  simulated.  Unlike  the  control  law  design,  the  simulation  is  based  on  the  full  nonlinear 
equations  of  motion  for  the  vehicle,  (3),  (4),  (5)  and  (7).  Typically,  the  initial  conditions 
consist  of  nonzero  values  of  the  lateral  deviation  y  and  heading  \\f. 

3.  Distance  in  X-Y  axis 

A  similar  procedure  is  used  to  simulate  the  vehicle  response  in  a  general  path 
in  the  X-Y  plane,  as  shown  in  Figure  2.  The  perpendicular  distance  y  from  the  vehicle 
to  the  desired  route  in  the  X-Y  plane  is  used  to  compute  the  commanded  heading  angle 
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\|/c.  The  difference  y-a,  where  a  is  the  angle  of  the  route  with  respect  to  the  X-axis,  is 
used  instead  of  the  heading  \j/  in  the  control  law.  The  above  computations  are  performed 
by  appropriate  coordinate  rotations  between  the  two  coordinate  systems. 

When  transiting  from  one  straight  line  path  to  the  next,  the  same  lookahead  distance 
xd  is  used  for  both  segments.  The  vehicle  switches  to  the  next  segment  when  it  gets 
within  a  specified  distance  from  the  terminal  way  point.  This  distance  is  measured  along 
the  x-axis  and  for  given  tc  and  xd,  and  it  should  increase  as  the  angle  a  for  the  next 
segment  increases.  Too  high  or  too  low  values  of  this  turning  distance  result  in  path 
overshoot  and  undesirable  oscillatory  response.  The  optimum  turning  distance  that  allows 
for  the  smooth  transition  between  consecutive  straight  line  paths  is  established  with  the 
aid  of  the  simulation  program  from  step  2  as  follows. 

For  a  fixed  initial  heading  y,  the  initial  deviation  y  is  varied  until  the  vehicle 
response  is  smooth  and  sufficiently  fast  with  no  path  overshoot.  The  process  is  repeated 
for  different  initial  conditions  in  \[/  and  a  curve  in  y  versus  \\f  is  constructed.  This  is 
shown  in  Figure  3  and  is  the  desirable  turning  distance  versus  turning  angle  curve.  The 
actual  curve  is  approximated  by  two  straight  lines  which  are  used  to  initiate  the  turn  in 
the  simulation  program.  A  copy  of  this  simulation  program  is  included  in  Appendix  B. 
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TURNING  DISTANCE  VERSUS  TURNING  ANGLE 
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Figure  3   Turning  distance  and  turning  angle 
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IV.   SIMULATION  RESULTS 


The  vehicle  parameters  used  in  the  simulation  are: 


X 

If 

= 

-0.0076 

X 

rr 

= 

0.0040 

X 

vr 

= 

0.0200 

X  s 

= 

-0.0010 

A 

vv 

= 

0.0530 

*v8 

= 

0.00173 

•*88 

= 

-0.1000 

n 

r 

= 

-0.0034 

n 

V 

= 

0.0012 

n 

r 

= 

-0.0160 

n 

V 

= 

-0.0074 

"« 

= 

-0.0130 

yr 

= 

0.0012 

y-v 

= 

-0.0550 

yr 

= 

0.0300 

V 

»    V 

= 

-0.1000 

y& 

= 

0.0270 

COy 

= 

0.3500 

w 

= 

12000 

lb. 

I 

= 

17.4 

ft- 

p 

= 

1.94 

slug/ft \ 

8 

= 

32.2 

fi./scc. 

I 

= 

10000 

ft'. 

V 

= 

0.000847 

ft2,  /sec 

The  simulation  begins  by  setting  \y  =  5  degrees,  xd  =  2  vehicle  lengths,  tc  =  5,  r 
=  0,  v  =  0  ft./sec,  u  =  5  ft./sec.  and  y  =  1.  When  the  vehicle  moves  to  a  distance  x  =  20 
then  the  simulation  stops.  The  route  of  vehicle  and  the  rudder  angle  (5)  that  vehicle  used 
during  simulation  are  shown  in  Figure  4.  The  heading  angle  (\]f)  and  commanded  heading 
(\j/c)  are  shown  in  Figure  5.  Yaw  velocity  and  sway  velocity  are  shown  in     Figure  6. 
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The  values  for  tc  and  xd  were  selected  based  on  the  results  of  the  previous  chapter. 
From  the  figures  it  can  be  seen  that  the  vehicle  response  is  very  fast  with  limited 
overshoot.  This,  of  course,  depends  heavily  on  the  initial  conditions  of  the  simulation.  The 
actual  heading  angle  converges  rather  rapidly,  after  the  initial  transients  have  died  out,  to 
the  commanded  heading  angle. 

The  second  series  of  simulations  was  performed  in  order  to  assess  the  capability 
of  the  control  and  guidance  law  to  change  course  and  keep  the  new  path.  Simulation 
parameters  were  xd  =  2,  tc  =  5  and  u  =  5  as  before.  Initial  conditions  for  the  simulations 
were  \|/  =  0,  r  =  0  and  v  =  0  with  the  initial  vehicle  position  at  (X^Yq)  =  (5,0)  in  the 
global  reference  frame.  The  first  straight  line  segment  is  determined  by  the  way  points 
(5,0)  and  (25,0)  and  the  second  by  (25,0)  and  (67.89,20).  For  this  route  the  corresponding 
course  change  is  25  degrees.  The  results  of  this  simulation  are  presented  in  Figure  7 
where  along  with  the  actual  vehicle  path,  a  side  path  at  distance  of  1  vehicle  length  off 
the  desired  path  is  shown.  This  corresponds  to  an  arbitrary  "  safety  path  band  "  for  the 
vehicle.  The  turning  distance  was  fixed  at  2  vehicle  lengths  throughout  the  simulations. 
From  Figure  7  it  can  be  seen  that  the  vehicle  turns  to  the  new  course  smoothly  with  no 
path  overshoot.  When  the  second  route  changes  to  (25,0)  and  (41.78,20)  which 
corresponds  to  50  degrees  course  change,  the  results  of  Figure  8  show  that  a  path 
overshoot  occurs  although  it  is  yet  not  serious  enough  according  to  the  artificial  safety 
criterion  described  above.  However,  when  the  second  route  changes  to  (25,0)  and 
(30.36,20)  which  corresponds  to  75  degrees  course  change,  significant  vehicle  oscillatory 
response  and  side  path  overshoot  occurs,  as  demonstrated  in  Figure  9. 
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The  above  simulations  demonstrate  the  need  for  adjustable  turning  distance; 
although  path  accuracy  is  obtained  regardless  of  the  value  of  the  turning  distance,  the 
transient  response  during  course  change  is  not  always  within  some  predetermined  safety 
bounds.  For  this  reason  we  employ  the  built-in  turning  distance  versus  turning  angle 
relationship  shown  in  Figure  3  and  repeat  the  simulations  for  the  aforementioned  three 
course  changes.  The  results  are  shown  in  Figures  10,  1 1  and  12,  where  it  can  be  seen  that 
the  vehicle  response  is  now  satisfactory  for  both  course  keeping  and  course  changing. 

Finally,  the  last  simulation  test  was  performed  in  order  to  establish  the  capabilities 
of  the  scheme  to  follow  a  general  path  in  the  horizontal  plane.  For  demonstration 
purposes  the  path  was  assumed  to  consist  of  the  way  points  (0,10),  (5,10),  (25,0),  (35,20), 
(50,20),  (70,10),  (50,-10)  and  (30,-10).  Straight  line  segments  were  assumed  as  the  desired 
paths  between  consecutive  way  points.  The  same  simulation  parameters  were  used  as  in 
the  previous  runs.  The  results  of  this  simulation  are  presented  in  Figures  13  for  adjustable 
turning  distance,  and  14  for  the  fixed  turning  distance  case.  It  can  be  clearly  seen  that 
when  the  turning  distance  is  function  of  turning  angle,  the  scheme  achieves  excellent  path 
keeping  characteristics  with  smooth  course  changes  and  minimal  path  overshoot. 
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Figure  4   Pursuit  navigation 
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Figure  8   Path  control 
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Figure  9    Path  control 
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Figure  10    Path  control 
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Figure  1 1    Path  control 
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Figure  12    Path  control 
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Figure  13    Pure  pursuit  navigation 
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V.   SUMMARY  AND  CONCLUSIONS 

The  principal  conclusions  of  this  work  can  be  summarized  as  follows: 

1 .  Orientation  control  law  can  be  used  in  order  to  provide  accurate  vehicle  path  keeping 
when  combined  with  an  appropriate  guidance  scheme. 

2.  Pure  pursuit  guidance  was  found  to  work  very  well  for  the  autonomous  underwater 
vehicle  case  and  its  simplicity  make  it  a  very  attractive  alternative  to  cross  track  error 
schemes. 

3.  A  built-in  turning  distance  versus  course  change  relationship  can  be  utilized  to  initiate 
the  turn  at  the  appropriate  time  in  order  to  avoid  path  overshoot  and  achieve  smooth  path 
transitions. 

4.  It  is  expected  that  the  added  robustness  that  heading  schemes  naturally  enjoy  will  aid 
in  maintaining  stability  in  cases  where  incomplete  and  inaccurate  vehicle  dynamic 
descriptions  are  available. 

Some  recommendations  for  further  research  are  as  follows: 
1 .  Comparative  studies  must  be  performed  with  other  orientation  guidance  schemes  such 
as  proportional  navigation  and  also  with  velocity  guidance  laws  in  order  to  ensure  that 
the  best  technique  is  ultimately  employed. 
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2.  Similar  studies  must  be  performed  in  the  vertical  plane.  Combined  with  the  horizontal 
plane  techniques  developed  in  this  work  and  with  propulsion  control  they  could  be 
utilized  to  provide  accurate  trajectory  following  in  3-D  space. 
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APPENDIX  A. 


1=17.425; 
inquire  xdl 
inquire  tc 
inquire  u 
xd=xd_l*l; 
aall=-0. 045380 
aal2=-0. 351190 
aa21=-0. 002795 
aa22=-0. 095680 
bbl=  0.0114  32; 
bb2=-0. 004273; 
OMEGA=( 10.0*U)/(TC*L) ; 
AD1=1 .  75*OMEGA; 
AD2=2.15*OMEGA**2; 
AD3=OMEGA**3; 
Al=BBl*U*U; 
Bl=BB2*U*U; 

Cl=-ADl-(AAll+AA2  2)*U; 
A2=(BB1*AA22-BB2*AA12 ) *U**3 ; 
B2=(BB2*AAll-BBl*AA21 )*U**3; 
Kl=AD3/( (BB2*AAll-BBl*AA21 )*U**3 ) ; 
C2=AD2-(AA11*AA2  2-AA12*AA21 )*U**2+BB2*U*U*Kl; 
K2=(C1*B2-C2*B1 )/( Al *B2-A2*Bl ) ; 
K3=(C2*A1-C1*A2)/(A1*B2-A2*B1 ) ; 
all  =  0 
al2  =  0 
al3  =  l 
al4  =  0 

a21=bbl*u*u*kl; 
a2  2=aall*u+bbl*u*u*k2; 
a2  3=aal2*u+bbl*u*u*k3; 
a2  4=bbl*u*u*kl/xd; 
a31=bb2*u*u*kl ; 
a3  2=aa21*u+bb2*u*u*k2; 
a3  3=aa2  2*u+bb2*u*u*k3; 
a3  4=bb2*u*u*kl/xd; 
a41=u 
a42  =  l 
a43  =  0 
a44  =  0 
a=[all 
a31 
eig( a) 


,al2 
,a32 


al3,al4 
a33,a34 


;a21 
;a41 


a22,a23,a24; 
a42,a43,a44] 
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APPENDIX  B. 


PROGRAM  SUB. FOR 
C 

C       PROUTTICHAI  SUWANDEE 
C      NAVAL  POSTGRADUATE  SCHOOL 
C      MARCH  1991 
C 

C      AUV  LINE-OF-SIGHT  NAVIGATION  AND  CONTROL 
C      VARIABLE  GAINS  INTERNALLY  COMPUTED 
C 

REAL  L,MASS,NRDOT,NVDOT,NR,NV,NDR 

REAL  IZ,NU,LLL,NSL,Kl,K2,K3 

DIMENSION  X(9) ,HH(9) ,VEC1(9) ,VEC2(9) ,TT (1000), YY (6,1000), 
*ALPHA( 10) ,XZ( 10) , YZ( 10 ) 
C 

C      LONGITUDINAL  HYDRODYNAMIC  COEFFICIENTS 
C 

PARAMETER ( XRR=4 . E-3 , XUDOT=-7 . 6E- 3 , XVR=2 . E-2 , XRDR=-1 . E-3 , 
1  XVV=5.3E-2,XVDR=1 . 7 E-3 , XDRDR=-1 .E-2) 

C 

C      LATERAL  HYDRODYNAMIC  COEFFICIENTS 
C 

PARAMETER ( YRDOT=l . 2 E-3 , YVDOT=-5 . 5E-2 , YR=3 . E-2 , YV=-1 . E-l , 
1  YDR=2 . 7E-2,CDY=3 . 5E-1 ) 

C 

C      YAW  HYDRODYNAMIC  COEFFICIENTS 
C 

PARAMETER (NRDOT=- 3. 4E-3,NVDOT=l . 2E-3,NR=-1 . 6E-2,NV=-7 .4 E-3, 
1  NDR=-1.3E-2) 

C 

C      MASS  CHARACTERISTICS  OF  THE  FLOODED  VEHICLE 
C 

PARAMETER(WEIGHT=12000. ,XG  =  0 . , I Z=l 0000 . , L=l 7 . 4 , RHO=l . 9 4 , 
1  G=32.2,NU=8.47E-4) 

C 

OPEN  ( 10 , FILE='SUB. IN' , STATUS= ' OLD ' ) 

OPEN  ( 11 , FILE*' SUB. OUT' , STATUS= ' OLD ' ) 
C 

READ  (10,*)  TARGET 

READ  (10,*)  TSIM, DELTA, IPRHT 

READ  (10,*)  PSI,R 

READ  (10,*)  U 

READ  (10,*)  TC,VC 
C 
C      NUMBER  OF  POSITION  OF  ROUTE  AND  POSITION  OF  VEHICLE 

C 

READ  (10,*)  N,XO,YO 
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c 

C      POSITION  OF  ROUTE  IN  X-Y  AXIS 
C 

DO  30  1=1, N 

READ  ( 10, *  )  XZ( I )  , YZ( I ) 
30   CONTINUE 

TARGET=TARGET*L 

TWOPI  =8.0*ATAN( 1 .0) 

PI     =0.5*TWOPI 

DO  999  M=1,N-1 

PSI1=0 

YTURN=0 

XYTURN=0 

PSI=PSI*PI/180. 0 
C 

C      MOVE  ORIGINE  OF  AXIS  TO  THE  FIRST  POINT  OF  THE  ROUTE 
C 

XF=XZ(M+1 )-XZ(M) 

IF  (XF.EQ.O)  XF=0. 0000001 

YF=YZ(M+1 )-YZ(M) 

XO=XO-XZ(M) 

IF  (XO.EQ.0)  XO=0. 0000001 

YO=YO-YZ(M) 
C 

C      ANGLE  OF  POSITION  OF  VEHICLE 
C 

ALPHA0=ATAN(ABS( YO/XO) ) 

IF  ( ( YO.GT.O ) .AND. (XO.LT.0) )  ALPHA0=PI-ALPHA0 

IF  ( (YO.LE.0) .AND. (XO.GT.0) )  ALPHA0=2*PI-ALPHA0 

IF  ( (YO.LE.0) .AND. (XO.LT.0) )  ALPHA0=PI+ALPHA0 
C 

C      ANGLE  OF  ROUTE 
C 

ALPHA1=ATAN(ABS( YF/XF) ) 

IF  ( ( YF.GT. 0 ) .AND. (XF.LT. 0 ) )  ALPHAl=PI-ALPHAl 

IF  ( ( YF.LE.0) .AND. (XF.GT.0) )  ALPHAl=2 *PI-ALPHAl 

IF  ( ( YF.LE. 0 ) .AND. (XF.LT.O ) )  ALPHAl=PI+ALPHAl 
C 

C      ANGLE  BETWEEN  VEHICLE  AND  ROUTE 
C 

BETA=(ALPHA0-ALPHA1 ) 
C 

C      DISTANCE  FROM  ORIGINE  TO  VEHICLE 
C 

RT=(XO**2+YO**2)**0.5 

C 

C       PROJECTED  DISTANCE  FROM  ORIGINE  TO  VEHICLE  ON  THE  ROUTE 

C 

XS=RT*COS( BETA) 
XC=(XS ) *COS(ALPHAl ) 
YC=(XS ) *SIN(ALPHA1 ) 
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c 

C      PERPENDICULAR  DISTANCE  OF  VEHICLE  TO  X-AXIS 
C 

YPOS=RT*SIN(BETA) 
C 

C      TOTAL  DISTANCE  ON  THE  ROUTE 
C 

TXD=(XF**2+YF**2  )**0  .  5 

XD=ABS(TXD-XS) 
C 

C       HEADING  ANGLE 
C 

IF(M.EQ.l)  PSI=PSI-ALPHA1 

IF  (PSI.GT.PI)  PSI=PSI-2*PI 

IF  (PSI.LT.-PI)  PSI=PSI+2*PI 

IF  (M.EQ.N-1 )  GO  TO  65 
C 

C      NEXT  HEADING  ANGLE 
C 

DUMY=YZ(M+2 )-YZ(M+l ) 

DUMX=XZ (M+2 ) -XZ ( M+l ) 

IF  (DUMX.EQ.O)  DUMX=0. 0000001 

ALP2=ATAN ( ABS ( DUMY/DUMX ) ) 

IF  (( DUMY.GT.0) .AND. (DUMX.LT.0) )  ALP2=PI-ALP2 

IF  (  (DUMY.LE.0)  .AND. (DUMX.GT.0)  )  ALP2=? *PI-ALP2 

IF  ( (DUMY.LE. 0 ) .AND. (DUMX.LT.0 ) )  ALP2=I I+ALP2 

PSIl=ALPHAl-ALP2 

IF  (PSIl.GT.PI)  PSIl=PSIl-2*PI 

IF  (PSI1.LT.-PI )  PSIl=PSIl+2*PI 

PSIl=PSIl*180/PI 
C 

C      TURNING  DISTANCE 
C 

IF(ABS(PSI1 ) .LE.45)  YTURN=ABS(PSIl/2  5) 

IF( ABS( PS  II )  .GT. 45)  YTURN=(ABS(PSI1 )-45) *0 . 3/5  +  1 .8 

XYTURN  =  YTURN/ABS( SIN ( PS 1 1  * PI/1 8  0 )  )-0.2 

IF(PSIl.EQ.O)  XYTURN=0.0 


C 


65    UC    =U 

OMEGA=( 10.0*U)/(TC*L) 
ADl   =1.75*OMEGA 
AD2   =2.15*OMEGA**2 
AD3   =OMEGA**3 


PISIM 

=TSIM/DELTA 

ISIM 

=PISIM 

ECHO 

=1 . 0/DELTA 

IECHO 

=IPRNT*20 

YAW 

=  0.0 

SWAY 

=  0.0 

V 

=  0.0 

DR 

=0.n 

R 

=R*FI/180 . 0 
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ISTART=1 
XPOS   =0.0 
YPOS   »YPOS*L 

C 

C       DEFINE  THE  LENGTH  X  AND  HEIGHT  HH  TERMS  FOR  THE  INTEGRATION 


C 


X(l)  =  -105.9/12. 

X(2)  =  -99.3/12. 

X(3)  =  -87.3/12. 

X(4)  =  -66.3/12. 

X(5)  =  72.7/12. 

X(6)  =  83.2/12. 

X(7)  =  91.2/12. 

X(8)  =  99.2/12. 

X(9)  =  103.2/12. 

HH(1)  =  0.00/12. 

HH(2)  =  8.24/12. 

HH(3)  =  19.76/12. 

HH(4)  =  29.36/12. 

HH(5)  =  31.85/12. 

HH(6)  =  27.84/12. 

HH(7)  =  21.44/12. 

HH(8)  =  12.00/12. 

HH(9)  =  0.00/12. 

MASS  =  WEIGHT/G 

Pl=MASS-0 . 5*RHO*L**3*XUDOT 
P3=MASS-0. 5*RHO*L**3*YVDOT 
P4=MASS*XG-0.5*RHO*L**4*YRDOT 
P5=IZ-0.5*RHO*L**5*NRDOT 
P6=MASS*XG-0. 5*RHO*L**4*NVDOT 
D  =P5*P3-P4*P6 

AA11  =  (P5*0. 5*RHO*L*L*YV-P4*0. 5*RHO* L* *  3 *NV ) /D 
AAl2=(P5*(-MASS+0 . 5*RHO*L**3*YR)-P4*(-MASS*XG 

1       +0. 5*RHO*L**4*NR) )/D 
AA21=( P3*0. 5*RHO*L**3*NV-P6*0.5*RHO*L*L*YV)/D 
AA2  2=( P3* (-MASS*XG+0. 5 *RHO* L* *  4 *NR ) -P6 * ( -MASS 

1       +0. 5*RHO*L**3*YR) )/D 
BBl=( P5*0. 5*RHO*L**2*YDR-P4*0.5*RHO*L**3*NDR)/D 
BB2=( P3*0 . 5*RHO*L**3*NDR-P6*0. 5*RHO*L**2*YDR )/D 

Al  =BB1*U*U 

Bl  =BB2*U*U 

Cl  =-ADl-(AAll+AA22)*U 

A2  =(-AAl2*BB2+AA22*BBl )*U**3 

B2  =(-AA21*BBl+AAll*BB2)*U**3 

Kl=AD3/( (BB2*AA11-BB1*AA21 )*U**3) 

C2  =AD2-(-AAl2*AA21+AAll*AA22)*U**2+BB2*U*U*Kl 

K2=(C1*B2-C2*B1 )/(Al*B2-A2*Bl ) 

K3=(C2*Al-Cl*A2)/(Al*B2-A2*Bl) 
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c 

c 
c 

c 
c 
c 


c 
c 
c 


c 
c 
c 


J  =0 
IJ  =  0 
JE  =  0 
OFF  =  0 

DRHAT=0.0 
DRBAR=0 . 0 

SIMULATION  BEGINS 

DO  100  1-1, I SIM 

CALCULATE  THE  DRAG  FORCE,  INTEGRATE  THE  DRAG  OVER  THE  VEHICLE 

DO  600  K=l,9 

UCF=V+X(K)*R 

SGN=1  .  0 

IF  (UCF.LT.0.0)  SGN=-1.0 

VEC1( K)=HH(K)*UCF*UCF*SGN 

VEC2( K)=HH( K) *UCF*UCF*SGN*X ( K ) 
600    CONTINUE 

CALL  TRAP(9,VEC1 , X , SWAY ) 
CALL  TRAP( 9,VEC2,X, YAW) 
SWAY=-0 . 5*RHO*CDY*SWAY 
YAW  =-0. 5*RHO*CDY*YAW 

FORCE  EQUATIONS 

FP2  =  -MASS*U*R+0. 5*RHO*L* * 3*YR*U*R+0 . 5*RHO*L*L* ( 
1  YV*U*V+YDR*U*U*DR)+SWAY 

FP3  =  -MASS*XG*U*R+0 . 5*RHO*L* *  4 *NR*U*R+0 . 5*RHO*L**3* 
1  (NV*U*V+NDR*U*U*DR)+YAW 

VDOT   =(P5*FP2-P4*FP3)/(P5*P3-P4*P6) 

RDOT   =(P6*FP2-P3*FP3)/(P4*P6-P3*P5) 

PSIDOT=R 

YDOT   =U*SIN(PSI )+V*COS( PSI ) +VC 

XDOT   =U*COS( PSI )-V*SIN( PSI ) 

FIRST  ORDER  INTEGRATION 

PSI  =PSI  +DELTA*PSIDOT 
V    =V    +DELTA*VDOT 
R    =R    +DELTA*RDOT 
XPOS=XPOS+ DELTA* XDOT 
YPOS=YPOS+ DELTA* YDOT 


YCTE=YPOS 

XAWAY=( XPOS-XD*L) 

IF  ( (XAWAY) .GE.-(XYTURN*L) )  OFF=l 
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c 

C         RUDDER  INPUT  CALCULATION 

C 

YA=ABS( YPOS) 

HDM=ATAN( ( YPOS )/( -TARGET ) ) 

DR=K1*( PSI-HDM)+K2*V+K3*R 


IF 

(  DR 

.GT. 

0. 

4) 

DR= 

0. 

4 

IF 

(DR 

.LE. 

-0. 

4) 

DR=- 

-0 

4 

PRINT    ] 

RESULTS 

c 
c 
c 

JE=JE+1 

IF  ( JE.NE.IECHO)  GO  TO  99 

WRITE  ( *  ,  *  )  '  XAWAY  - '  ,  XAWAY/L 

JE=0 
99    J=J+1 

IF  (J.NE.IPRNT)  GO  TO  100 

IJ=IJ+1 

TIME=I*DELTA 

XP=XPOS/L 

YP=YPOS/L 

XI=XZ(M)+XC+XP*COS( -ALPHAl ) +YP*SIN ( -ALPHAl ) 

YI=YZ(M)+YC+YP*COS( -ALPHAl ) -XP* SIN ( -ALPHAl ) 

WRITE  (11,*)  XI, YI 

0  =  0 

IF  (OFF.EQ.l)  GO  TO  500 
100     CONTINUE 
500     PSI=PSI1 

XO=XI 

YO=YI 
999     CONTINUE 

STOP 

END 
C 

SUBROUTINE  TRAP ( N , A , B , OUT ) 
C 

C      NUMERICAL  INTEGRATION  ROUTINE  USING  THE  TRAPEZOIDAL  RULE 
C 


DIMENSION  A( 1 ) ,B( 1 ) 

Nl=N-l 

OUT=0 . 0 

DO  1  1=1, Nl 

OUT1=0.5*(A( I)+A(I+1))*(B(I+1)-B(I)) 

OUT  =OUT+OUTl 
CONTINUE 
RETURN 
END 
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